/***********************************************************************
Copyright 2019 Wuhan PS-Micro Technology Co., Itd.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
***********************************************************************/

#include <string>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
using namespace std;

bool IsArmToMove = 0;

// 关节状态发布者及订阅者
ros::Subscriber JointStateSuber_Moveit; 

//关节角赋值
std::vector<double> joint_group_positions(5);

// 收到关节信息的回调函数
void JointsStatusCallback(const sensor_msgs::JointState& msg)
{
    for(int i=0;i<7;i++)
    {
        joint_group_positions[i] = msg.position[i];
    }
    IsArmToMove = 1;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "mrobot_moveit_controller");
    ros::AsyncSpinner spinner(1);
    spinner.start();

    ros::NodeHandle n;

    moveit::planning_interface::MoveGroupInterface arm("manipulator");

    //获取终端link的名称
    std::string end_effector_link = arm.getEndEffectorLink();

    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "base_link";
    arm.setPoseReferenceFrame(reference_frame);

    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);

    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);

    //设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.8);
    arm.setMaxVelocityScalingFactor(0.5);

    // 订阅关节信息
    JointStateSuber_Moveit =  n.subscribe("xrobot_controller/joint_states", 100, JointsStatusCallback);

    // 设置循环的频率
    ros::Rate loop_rate(10);

    // 循环等待
    // while(!IsArmToMove)
    while(ros::ok())
    {
        // 延时等待
        loop_rate.sleep();
        // 查看中断函数
        ros::spinOnce();
        //机械臂移动到目标点
        arm.setJointValueTarget(joint_group_positions);
        arm.move();
    }
    //机械臂移动到目标点
    // arm.setJointValueTarget(joint_group_positions);
    // arm.move();
    

    
    sleep(1);

    ros::shutdown(); 

    return 0;
}

